#ifndef IRSENSORS_H
#define IRSENSORS_H

#include <WProgram.h>
#include <inttypes.h>

#define ID_ADC_LINE      2
#define ID_LEFTRIGHT    12
#define ID_FRONTREAR    13
#define ID_ADC_DISTANCE  3

#define ID_EXTRA_SENSOR_L 4
#define ID_EXTRA_SENSOR_R 5

#define ID_EXTRA_SENSOR_SELECT_1 4
#define ID_EXTRA_SENSOR_SELECT_2 2

#define LEFT          0
#define RIGHT         1

#define NUM_E_IR_SENSORS  8
#define NUM_IR_SENSORS    3

class ExtraIRSensors
{
public:
	ExtraIRSensors();
	int readSensor(int, int);
	int* getDistance();
	void setMinDistance(int);
        int getMinDistance();
	
private:
	int _defaultDistance[NUM_E_IR_SENSORS];	// calibrated default distances of for sensors
	int _distance[NUM_E_IR_SENSORS]; //The actual distance in relation to the default distance
	int _maxDistance; //max value the distance sensors can obtain
	int _minDistance; //At which change in distance measurement the robot should react
};

class IRSensors
{
public:
	IRSensors();
	int readSensor(int);
	int* getDistance();
	void setMinDistance(int);
        int getMinDistance();
	
private:
	int _defaultDistance[NUM_IR_SENSORS];	// calibrated default distances of for sensors
	int _distance[NUM_IR_SENSORS]; //The actual distance in relation to the default distance
	int _maxDistance; //max value the distance sensors can obtain
	int _minDistance; //At which change in distance measurement the robot should react
};


ExtraIRSensors::ExtraIRSensors()
{
	/*pinMode(ID_LEFTRIGHT, OUTPUT);
	digitalWrite(ID_LEFTRIGHT, HIGH);

	pinMode(ID_FRONTREAR, OUTPUT);
	digitalWrite(ID_FRONTREAR, HIGH);  */

	
	pinMode(ID_EXTRA_SENSOR_SELECT_1, OUTPUT);        
	pinMode(ID_EXTRA_SENSOR_SELECT_2, OUTPUT);
	
	_maxDistance = 950;
	_minDistance = 400;
	
	for(int i = 0; i < NUM_E_IR_SENSORS; i++)
	{
		_defaultDistance[i] = _maxDistance;  
	}
}

int ExtraIRSensors::readSensor(int side, int id) {
    digitalWrite(ID_EXTRA_SENSOR_SELECT_1, (id)%2);
    digitalWrite(ID_EXTRA_SENSOR_SELECT_2, (id)/2);
  //  Serial.println((id-1)%2);   
  //  Serial.println((id-1)/2);   
    delay(1);
    if (side == LEFT) return analogRead(ID_EXTRA_SENSOR_L);
    else return analogRead(ID_EXTRA_SENSOR_R);
}

int* ExtraIRSensors::getDistance()
{
   //The measured distance from the sensor
   int measureDistance[NUM_E_IR_SENSORS];
   
   for(int i = 0; i < 2; i++)
   {
     for(int j = 0; j < 4; j++)
     {
       //which sensor is selected
       int sensorID = 4 * i + j;
       
       measureDistance[sensorID] = readSensor( i, j);  
       _distance[sensorID] = abs(_defaultDistance[sensorID] - measureDistance[sensorID]);
       
     }
   }
   return _distance;
}


void ExtraIRSensors::setMinDistance(int min)
{
	_minDistance = min;
}

int ExtraIRSensors::getMinDistance()
{
  return _minDistance;
}

IRSensors::IRSensors()
{
	pinMode(ID_LEFTRIGHT, OUTPUT);
	digitalWrite(ID_LEFTRIGHT, HIGH);

	pinMode(ID_FRONTREAR, OUTPUT);
	digitalWrite(ID_FRONTREAR, HIGH); 
	
	_maxDistance = 950;
	_minDistance = 400;
	
	for(int i = 0; i < NUM_IR_SENSORS; i++)
	{
		_defaultDistance[i] = _maxDistance;  
	}
}

int IRSensors::readSensor(int side)
{
   switch(side)
   {
     case 0:
       digitalWrite(ID_FRONTREAR, LOW);
       digitalWrite(ID_LEFTRIGHT, LOW);
       break;
     case 1:
       digitalWrite(ID_FRONTREAR, HIGH);
       digitalWrite(ID_LEFTRIGHT, LOW);
       break;
     case 2:
       digitalWrite(ID_FRONTREAR, LOW);
       digitalWrite(ID_LEFTRIGHT, HIGH);
       break; 
   }
   delay(1);
   return analogRead(ID_ADC_DISTANCE);
}

int* IRSensors::getDistance()
{
   int measureDistance[NUM_IR_SENSORS];//The measured distance from the sensor
   
   for(int i = 0; i < NUM_IR_SENSORS; i++)
   {
     measureDistance[i] = readSensor(i);  
     _distance[i] = _defaultDistance[i] - measureDistance[i];
   }
   return _distance;
}


void IRSensors::setMinDistance(int min)
{
	_minDistance = min;
}

int IRSensors::getMinDistance()
{
  return _minDistance;
}

#endif
